import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import (
    ExecuteProcess,
    RegisterEventHandler,
)
import xacro
from launch_ros.actions import Node
from launch.event_handlers import OnProcessExit


def generate_launch_description():

    pkg_robot_description = get_package_share_directory("robot_description" )

    robot_xacro_path = os.path.join(
        pkg_robot_description,
        "resource",
        "xacro",
        "robot.xacro",
    )
    doc = xacro.parse(open(robot_xacro_path))
    xacro.process_doc(doc)
    robot_description_config = doc.toxml()
    robot_description = {"robot_description": robot_description_config}

    ld = LaunchDescription()

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        output="screen",
        parameters=[robot_description],
    )
    spawn_entity = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=[
        "-topic", "robot_description",
        "-entity", "combined_robot",
        # "-x", "11.0",         # 初始 X 坐标
        # "-y", "-7.0",         # 初始 Y 坐标
        "-z", "0.5",         # 初始 Z 高度
        # "-Y", "-1.57"         # 初始 yaw 朝向（单位为弧度）
    ],
        output="screen",
    )

    load_joint_state_controller = ExecuteProcess(
        cmd=[
            "ros2",
            "control",
            "load_controller",
            "--set-state",
            "active",
            "joint_state_broadcaster",
        ],
        output="screen",
    )

    # 系统是如何知道有fairino10_v6_group_controller这个控制器的存在？
    load_joint_trajectory_controller = ExecuteProcess(
        cmd=[
            "ros2",
            "control",
            "load_controller",
            "--set-state",
            "active",
            "fairino10_v6_group_controller",
        ],
        output="screen",
    )

    # 监听 spawn_entity，当其退出（完全启动）时，启动load_joint_state_controller？
    close_evt1 = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=spawn_entity,
            on_exit=[load_joint_state_controller],
        )
    )
    # 监听 load_joint_state_controller，当其退出（完全启动）时，启动load_joint_trajectory_controller？
    # moveit是怎么和gazebo这里提供的action连接起来的？？
    close_evt2 = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=load_joint_state_controller,
            on_exit=[load_joint_trajectory_controller],
        )
    )
    # ld.add_action(close_evt1)
    # ld.add_action(close_evt2)
    ld.add_action(robot_state_publisher)
    ld.add_action(spawn_entity)

    return ld
